/*

sybon_ls_modbus.c
designed sybon_ls motor driver drive,control motor action , config and init  modbus protocol
designed by tianchunyu

------------------------
2SYNRON_SUCCESS20-04SYNRON_COMMUN_ERR0 16:34:59
create file

------------------------
*/
/***********************************coil read/write*************************/
#include"synron_ls_modbus.h"
 SYNRO_handler synro_handler_A = {0};   //A电机为左B电机为右  都为小车行走电机
 SYNRO_handler synro_handler_B = {0};

 SYNRO_handler synro_lifter_handler_A = {0};   //A电机为左B电机为右  都为电缸推杆电机
 SYNRO_handler synro_lifter_handler_B = {0}; 
 
 SYNRO_handler synro_brush_handler = {0};   //电机为刷子电机
 SYNRO_handler synro_brush_hinge_hander = {0}; //为控制刷子电机与水平角度的电机 铰链
 /*
 synron:synron_write_register
 param:
     handler:object 
     addr:reginster address
     state:synron 
 return:
     0 =success
     SYNRON_COMMUN_ERR:modbus return error
 */

 int synron_write_register(const SYNRO_handler *handler,u16 addr,u16 val){
    int sta=0;
    MB_data modbus;
    int ret =0;
    //保证多线程通信的可靠性
    u8 rec_data[10];
    u8 rec_len = 0;
    u16 value =0;
    u8 count=5;
    value = val;  ////
    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_hold_w;       //功能码对应的0x10
    //modbus.fun_code = 0x06;     //
    modbus.start_addr=addr;
    modbus.wBuf=&value;
    modbus.rwLen=1;             //寄存器个数
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    while(count--)
    {
        sta=xp_modbus_ask(handler->port,&modbus);
        if(sta==0)
        {
            return 0;
        }
    }
    if(sta!=0)
    {
        return -1;
    }
}

/*
unuseful in this driver!!!
synron motor read coil
param；
    handler:motor object
    addr:reginster address
return:coil state,the value
*/
 int  synron_read_coil(SYNRO_handler *handler,u16 addr){
    int sta=0;
    MB_data modbus;
    u8 rec_data[10];
    u8 rec_len;
    u16 val;
    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_coil_r;
    modbus.start_addr=addr;
    modbus.rwLen=1;
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    sta=xp_modbus_ask(handler->port,&modbus);
    if(sta!=SYNRON_SUCCESS)return SYNRON_COMMUN_ERR;
    memcpy(&val,rec_data,2);
    LOG_SYNRON("read len=%d,data=%d",rec_len,rec_data[0]);
    return val;
}
/*
synron motor read hold
param；
    handler:motor object
    addr:reginster address
return:s16 reginster value 
*/
 s16  synron_read_hold(SYNRO_handler *handler,u16 addr){
    int sta=0;
    MB_data modbus;
    u8 rec_data[10];
    u8 rec_len;
    s16 val;
    u8 count =5;
    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_hold_r;
    modbus.start_addr=addr;
    modbus.rwLen=1;
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    while(count--)
    {
        sta=xp_modbus_ask(handler->port,&modbus);
        if(sta==0)
        {
            val = ((rec_data[0]<<8)|(rec_data[1]));
            LOG_SYNRON("read len=%d,rec_data: 0x%x,0x%x  return val %d ",rec_len,rec_data[0],rec_data[1],val);
            return val;
        }
    }
    if(sta!=0)
    {
        return READADD_16_ERR;
    }
}

/*
synron_read_reginster
param:
    handler:motor object
    addr:reginster address
    rec_data:save recive data
    rec_len:save recive length
return:
    0=success
    -1=modbus return error
*/
int synron_read_reginster(const SYNRO_handler *handler,u16 addr,u16 read_len,u8 *rec_data,u8 *rec_len){
    int sta=0;
    MB_data modbus;
    u8 count=5;
    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_hold_r;
    modbus.start_addr=addr;
    modbus.rwLen=read_len;
    modbus.resBuf=rec_data;
    modbus.resLen=rec_len;
    while(count--){
        sta=xp_modbus_ask(handler->port,&modbus);
        if(sta==0)return 0;
    }
    if(sta!=0)return -1;
}

/*
synron motor enable state set
param:
    handler:motor handler
    en:0=disable,1=enable
return:
    0=success
    1:need't action
    SYNRON_COMMUN_ERR=modbus return error
*/
int synron_en(SYNRO_handler *handler,SYNRON_TYPE en){
   if(handler->en!=en){
        handler->en=en;
        return synron_write_register(handler,PARAMETER_SERVO_ENABLE,en);
    }
    return SYNRON_NOR_ERR;
}

/*
synron motor communication  mode set
param:
    handler:motor handler
    en:1 nomal  2 Modbus 3 Can
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/
int synron_mode_commset(SYNRO_handler *handler,u16 en){
    if(en < 1 || en > 3){
     LOG_SYNRON("[%s_%d]param en is wrong ! it's value should be from 1 to 3",__FILENAME__,__LINE__);
     return SYNRON_COMMUN_ERR;
    }
    return synron_write_register(handler,PARAMETER_MODE_WORK,en);
}

/*
synron motor drive  mode set
param:
    handler:motor handler
    en:1: torque operation;2: speed operation;3: incremental position operation;
    4: absolute position operation
    
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/
int synron_mode_driveset(SYNRO_handler *handler,u8 en){
    if(en > 7){
     LOG_SYNRON("param en is wrong ! it's value should be from 0 to 4");
     return SYNRON_COMMUN_ERR;
    }
    return synron_write_register(handler,PARAMETER_MODE_M_C_CONTROL,en);
}

/*
synron_setemg_pinnum(SYNRO_handler *handler,u8 pinnum)
param:
    handler:motor handler
    pinnum:input pin number 
    1: internal invalid;0: internally valid;-1~-4: determined by the digital 
    input signal port IN1~4 of CN2.The emergency stop signal is active at low level 
    for emergency stop, when the motor windings are cut off rapidly
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/
int synron_setemg_pinnum(SYNRO_handler *handler,s8 pinnum)
{
    int ret =0;
    if(pinnum <1 || pinnum >4)
    {
        LOG_SYNRON("pinnum must be range from 1 to 4");
        return SYNRON_COMMUN_ERR;
    }
    ret = synron_write_register(handler,PARAMETER_STOP_EMG,-pinnum);
    if(ret !=SYNRON_SUCCESS )
    {
        LOG_SYNRON("synron_write_register(handler,PARAMETER_STOP_EMG,-pinnum) fail !!");
        return SYNRON_COMMUN_ERR;
    }  
    return SYNRON_SUCCESS;
}



/*
synron_speedmode_setvalue 
param:
    handler:motor handler
    value:In MODBUS speed mode, speed command. Unit: RPM its can be negative and its direction is negative
    
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/



int synron_speedmode_setvalue(SYNRO_handler *handler, s16 value){
    handler->motor_speed = value;     //unit prm
    return synron_write_register(handler,COMMAND_NUM_SET_SPEED,value);
}

/*
synron_speedmode_motorstop 
The motor stop
param:
    handler:motor handler    
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedmode_motorstop(SYNRO_handler *handler){
    return synron_write_register(handler,COMMAND_NUM_SET_SPEED,0);
}

/*
synron_speedmode_motor_acc 
The motor accelerates to the specified speed
param:
    handler:motor handler
    acc: acceleration
    destr :Target rotation speed
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedmode_motor_acc(SYNRO_handler *handler,u16 acc,s16 destr ){
    u8 mode =0;
    s16 curv = 0;
    u8 ret = 0;
    if(acc>1000){
        LOG_SYNRON("=====>the acc is too high acc %d ",acc);
        return SYNRON_COMMUN_ERR;
    }
    mode = synron_read_hold(handler,PARAMETER_MODE_M_C_CONTROL);
    if(mode != 2){
        LOG_SYNRON("=====>the mode is not  speed mode and its mode %d",mode);
        return SYNRON_COMMUN_ERR;
    }
    curv = synron_get_speedvalue(handler);
    if(destr <= curv){
        LOG_SYNRON("=====>destr %d <= curv %d  and logic is error!!\n",destr,curv); 
        return SYNRON_COMMUN_ERR;    
    }
    while(curv < destr - curv){
        curv = curv + acc;      
        ret =  synron_speedmode_setvalue(handler, curv);
        if(ret != SYNRON_SUCCESS){
            LOG_SYNRON("synron_speedmode_setvalue \n");
            return SYNRON_COMMUN_ERR; 
        }
        aos_msleep(900);
                          
    }
        ret = synron_speedmode_setvalue(handler, destr);
        if(ret != SYNRON_SUCCESS){
            LOG_SYNRON("synron_speedmode_setvalue \n");
            return SYNRON_COMMUN_ERR; 
        }
   return SYNRON_SUCCESS;
}



/*
synron_speedmode_motor_dec 
The motor slow down to the specified speed
param:
    handler:motor handler
    acc: deceleration
    destr :Target rotation speed
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedmode_motor_dec(SYNRO_handler *handler,u16 acc,s16 destr ){
    u8 mode =0;
    s16 curv = 0;
    u8 ret = 0;
    if(acc>1000){
        LOG_SYNRON("=====>the acc is too high acc %d ",acc);
        return SYNRON_NOR_ERR;
    }
    mode = synron_read_hold(handler,PARAMETER_MODE_M_C_CONTROL);
    if(mode != 2){
        LOG_SYNRON("=====>the mode is not  speed mode and its mode %d",mode);
        return SYNRON_NOR_ERR;
    }
    curv = synron_get_speedvalue(handler);
    if(destr >= curv){
        LOG_SYNRON("=====>destr %d >= curv %d  and logic is error!!\n",destr,curv); 
        return SYNRON_NOR_ERR;    
    }
    while(curv >destr +acc ){
        curv = curv - acc;   
        ret =  synron_speedmode_setvalue(handler, curv);
        if(ret != SYNRON_SUCCESS){
            LOG_SYNRON("synron_speedmode_setvalue \n");
            return SYNRON_COMMUN_ERR; 
        }       
         aos_msleep(900);
    }
        ret = synron_speedmode_setvalue(handler, destr);
        if(ret != SYNRON_SUCCESS){
            LOG_SYNRON("synron_speedmode_setvalue \n");
            return SYNRON_COMMUN_ERR; 
        }
   return SYNRON_SUCCESS;
    
}


/*
synron_speedmode_motor_slow_stop 
The motor stop slowly
param:
    handler:motor handler
    absacc: acceleration or deceleration
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/
int synron_speedmode_motor_slow_stop(SYNRO_handler *handler,u16 acc)
{
    
    s16 curv = 0;
    u8 ret = 0;
    if(acc>1000){
        LOG_SYNRON("=====>the acc is too high acc %d ",acc);
        return SYNRON_COMMUN_ERR;
    }
    curv = synron_get_speedvalue(handler);
    if(curv >0)
    {
         ret = synron_speedmode_motor_dec(handler,acc,0);
         if(ret != SYNRON_SUCCESS)
         {
             LOG_SYNRON("=====>synron_speedmode_motor_dec fail !");
             return SYNRON_COMMUN_ERR;
         }
    }
    else if(curv <0)
    {
        ret = synron_speedmode_motor_acc(handler,acc,0);
        if(ret != SYNRON_SUCCESS)
         {
             LOG_SYNRON("=====>synron_speedmode_motor_acc fail !");
             return SYNRON_COMMUN_ERR;
         }
    }
    else
    {
       ret = synron_speedmode_setvalue(handler, 0);
       if(ret != SYNRON_SUCCESS)
         {
             LOG_SYNRON("=====>synron_speedmode_setvalue fail !");
             return SYNRON_COMMUN_ERR;
         }
    }
    return SYNRON_SUCCESS;

    
}
/*
synron_speedmode_acceleratetime 
When the speed instruction changes +1000 RPM, the time required for the feedback velocity to follow the instruction velocity

param:
    handler:motor handler
    value:In MODBUS speed mode, accelerate command. Unit: ms
    When the speed instruction is given for the step, the motor is started smoothly by setting the acceleration and deceleration time.
    The default value 100 ms
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedmode_acceleratetime(SYNRO_handler *handler,u16 value){
    return synron_write_register(handler,PARAMETER_TIME_ACCELERATE,value);
}
/*
synron_speedmode_deceleratetime 
When the speed instruction changes +1000 RPM, the time required for the feedback velocity to follow the instruction velocity

param:
    handler:motor handler
    value:In MODBUS speed mode, accelerate command. Unit: ms
    When the speed instruction is given for the step, the motor is started smoothly by setting the acceleration and deceleration time.
    The default value 100 ms
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedmode_deceleratetime(SYNRO_handler *handler,u16 value){
    return synron_write_register(handler,PARAMETER_TIME_DECELERATION,value);
}




/*
synron_speedloop_load_rotational 
param:
    handler:motor handler
    value:100~5000  unit 1%  
    Coefficient of moment of inertia of load
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedloop_load_rotational(SYNRO_handler *handler,u16 value){
    return synron_write_register(handler,PARAMETER_MOMENT_FAC,value);
}


/*
synron_speedloop_adjustkv1 
param:
    handler:motor handler
    value:10~5000 0.01% 
    Velocity loop 1 proportional gain Kv1
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedloop_adjustKv1(SYNRO_handler *handler,u16 value){
    return synron_write_register(handler,PARAMETER_SPEED_CON_KV1,value);
}

/*
synron_speedloop_adjustTi1 
param:
    handler:motor handler
    value:1~10000  unit 0.1 ms 
    The integral time constant Ti: 0 at velocity loop 1 means the integral control is cancelled
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedloop_adjustTi1(SYNRO_handler *handler,u16 value){
    return synron_write_register(handler,PARAMETER_SPEED_CON_KV1,value);
}

/*
synron_speedloop_adjustKv2 
param:
    handler:motor handler
    value:10~5000 0.01% 
    Velocity loop 1 proportional gain Kv1
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedloop_adjustKv2(SYNRO_handler *handler,u16 value){
    return synron_write_register(handler,PARAMETER_SPEED_CON_KV1,value);
}

/*
synron_speedloop_adjustTi2 
param:
    state:motor handler
    value:1~10000  unit 0.1 ms 
    The integral time constant Ti: 0 at velocity loop 1 means the integral control is cancelled
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_speedloop_adjustTi2(SYNRO_handler *handler,u16 value){
    return synron_write_register(handler,PARAMETER_SPEED_CON_KV1,value);
}


/*
synron_get_speedvalue
param；
    handler:motor object
return:speed value  unit:rpm
*/
int synron_get_speedvalue(SYNRO_handler *handler){
    
    return synron_read_hold(handler,STATE_MOTOR_SPEED);
}

/*
获得电机有效转矩率
synron_get_validtor_rate_value
param；
    handler:motor object
return:
    Effective torque of motor
*/
int synron_get_validtor_rate_value(SYNRO_handler *handler){
    
    return synron_read_hold(handler,STATE_MOTOR_VALID_TOR_RATE);
}




/*
synron_position_setvalue 
param:
    handler:motor handler
    value:In absolute positon ,  PULE
    time : unit (ms) the time it takes to reach the location for one use
    if value =0 time = 0 it can stop cunrenttly！！
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_position_setvalue(SYNRO_handler *handler,int value,u16 time){
    int sta=0;
    MB_data modbus;
    s16 ret = 0;
    u16 data[3]={0};
    u8 rec_data[10];
    u8 rec_len;
    data[0] = time;
    data[1] = (u16)value;
    data[2] = (u16)(value>>16);
    LOG_SYNRON("ret is %d  time: %d value is %d data[0]_low 0x%x data[1]_hi 0x%x",ret,time,value,data[0],data[1]);
    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_hold_w;       //功能码对应的0x10//
    modbus.start_addr=COMMAND_SPE_BASE_ADR;
    modbus.wBuf=data;
    modbus.rwLen=3;             //寄存器个数
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    sta=xp_modbus_ask(handler->port,&modbus);
    if(sta!=0){
    LOG_SYNRON("line is %d and sta is %d ",__LINE__,sta);
    return SYNRON_COMMUN_ERR;
    }
    else{
    return SYNRON_SUCCESS;
    }
}
/*
synron_position_re_setvalue 
param:
    handler:motor handler
    encode_vaule:In absolute positon encode reset its vaule,its for encode value
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_position_re_encode_setvalue(SYNRO_handler *handler,s32 encode_vaule){
     int sta=0;
    MB_data modbus;
    s16 ret = 0;
    u16 data[2]={0};
    u8 rec_data[10];
    u8 rec_len;
    u8 count=5;
    data[0] = (u16)encode_vaule;
    data[1] = (u16)(encode_vaule>>16);
    LOG_SYNRON("=====>encode_vaule is %d data[0]_low 0x%x data[1]_hi 0x%x",encode_vaule,data[0],data[1]);
    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_hold_w;       //功能码对应的0x10//
    modbus.start_addr=COMMAND_NUM_SET_LOW_COORDINATE;
    modbus.wBuf=data;
    modbus.rwLen=2;             //寄存器个数
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    while(count--)
    {
        sta=xp_modbus_ask(handler->port,&modbus);
        if(sta==0)
        {
            return 0;
        }
    }
    if(sta!=0)
    {
        return SYNRON_COMMUN_ERR;
    }
}

/*
synron_position_re_locacmd_setvalue 
param:
    handler:motor handler
    encode_vaule:In absolute positon encode reset its vaule,its for location command
    if value =0 time = 0 it can stop cunrenttly！！
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_position_re_locacmd_setvalue(SYNRO_handler *handler,s32 encode_vaule){
     int sta=0;
    MB_data modbus;
    s16 ret = 0;
    u16 data[2]={0};
    u8 rec_data[10];
    u8 rec_len;
    data[0] = (u16)encode_vaule;
    data[1] = (u16)(encode_vaule>>16);
    LOG_SYNRON("=====>encode_vaule is %d data[0]_low 0x%x data[1]_hi 0x%x",encode_vaule,data[0],data[1]);
    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_hold_w;       //功能码对应的0x10//
    modbus.start_addr=COMMAND_NUM_SET_LOW_POS;
    modbus.wBuf=data;
    modbus.rwLen=2;             //寄存器个数
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    sta=xp_modbus_ask(handler->port,&modbus);
    if(sta!=SYNRON_SUCCESS){
    LOG_SYNRON("line is %d and sta is %d ",__LINE__,sta);
    return SYNRON_COMMUN_ERR;
    }
    else{
    return SYNRON_SUCCESS;
    }
}


/*
synron_posloop_setfeedbackrate 
param:
    handler:motor handler
    value:0~100 unit 1%
    Feedback coefficient in front of position loop
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/


int synron_posloop_setfeedbackrate(SYNRO_handler *handler,u8 value){
    if(value > 100){
        LOG_SYNRON("set value wrong!synron_posloop_setfeedbackrate");
        return SYNRON_COMMUN_ERR;
    }
    return synron_write_register(handler,PARAMETER_POSLOOP_VEC,value);

}

/*
synron_posloop_setfeedback_filtime 
param:
    handler:motor handler
    value:0~6400 unit 1%
    Position loop velocity feedforward low pass filter time constant
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_posloop_setfeedback_filtime(SYNRO_handler *handler,u16 value){
    if(value > 6400){
        LOG_SYNRON("set value wrong!synron_posloop_setfeedbackrate");
        return SYNRON_COMMUN_ERR;
    }
    return synron_write_register(handler,PARAMETER_POSLOOP_LOW_FIL_VEC,value);

}

/*
synron_posloop_Kp1 
param:
    handler:motor handler
    value:1~10000 unit 0.01
    Position loop 1 proportional gain Kp1 1 ~ 10000 0.0
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/

int synron_posloop_Kp1(SYNRO_handler *handler,u16 value){
    if(value > 10000){
        LOG_SYNRON("set value wrong!synron_posloop_setfeedbackrate");
        return SYNRON_COMMUN_ERR;
    }
    return synron_write_register(handler,PARAMETER_POSLOOP_KP1,value);

}

/*
synron_posloop_Kp2 
param:
    handler:motor handler
    value:1~10000 unit 0.01
    Position loop 2 proportional gain Kp2 1 ~ 10000 0.0
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
*/
int synron_posloop_Kp2(SYNRO_handler *handler,u16 value){
    if(value > 10000){
        LOG_SYNRON("set value wrong!synron_posloop_setfeedbackrate");
        return SYNRON_COMMUN_ERR;
    }
    return synron_write_register(handler,PARAMETER_POSLOOP_KP2,value);

}

/*
synron_set_default_rotation

param:
    handler:motor handler
    value: 0 or 1 0 is default!
    Facing the direction of the motor axis, 0: counterclockwise direction is positive rotation of the motor;1: clockwise direction is positive rotation of the motor
return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
PARAMETER_ROTA_DIRECTION
*/

int synron_set_default_rotation(SYNRO_handler *handler,u8 value){
    if(value!=1 && value!=0){
     LOG_SYNRON("set value is wrong!!!! synron_set_default_rotation");
     return SYNRON_COMMUN_ERR;
     }
    return synron_write_register(handler,PARAMETER_ROTA_DIRECTION,value);
    
}

/*
#define STATE_MOTOR_FAULT_CODE              (STATE_BASE_ADR + 0x36)    //当前扫描的故障代码
#define STATE_MOTOR_SERIOUS_ERROR_CODE      (STATE_BASE_ADR + 0x38)    //当前扫描的严重错误代码
#define STATE_MOTOR_SERIOUS_WARNING_CODE    (STATE_BASE_ADR + 0x39)    //当前扫描的严重故障代码

synron_get_curfaultcode
Current scanned system failure code
get the critical error code
param:
     handler:motor handler
return 
    currently  code 
*/

int synron_get_curfault_code(SYNRO_handler *handler){
    return synron_read_hold(handler,STATE_MOTOR_FAULT_CODE);
}

/*
synron_get_curserious_waring_code
Get the critical waring code
param:
     handler:motor handler
return 
    currently  code 
*/

int synron_get_curserious_waring_code(SYNRO_handler *handler){
    return synron_read_hold(handler,STATE_MOTOR_SERIOUS_WARNING_CODE);
}


/*
synron_get_curserious_error_code
Get the critical error code
param:
     handler:motor handler
return 
    currently  code 
*/

int synron_get_curserious_error_code(SYNRO_handler *handler){
    return synron_read_hold(handler,STATE_MOTOR_SERIOUS_ERROR_CODE);
}

/*
synron_get_abs_pulsevalue
param；
    handler:motor object
return: abs position mode accumulation pulse value  unit:rpm
*/

int synron_get_abs_pulsevalue(SYNRO_handler *handler){
    int sta=0;
    u8 rec_data[20];
    u8 rec_len;
    int encode = 0;
    sta=synron_read_reginster(handler,STATE_MOTOR_ABS_POS_LOW_ACCVALUE,2,rec_data,&rec_len);
    if(sta != 0)
    {
        LOG_SYNRON("synron_get_abs_pulsevalue error  ");
        return READADD_32_ERR ;//错误
    }
    encode =rec_data[2]<<24|rec_data[3]<<16|rec_data[0]<<8|rec_data[1];
    return encode;

}



/*
synron_init
param:
    hander:    synron hander
    serial 1:  is A  2 is B 
    slave_id:  is modbus adrress
    port:      uart port
    speed:      modbus uart speed
    return:
    0=success
    SYNRON_COMMUN_ERR=modbus return error and other error
    

*/
int synron_hander_init(SYNRO_handler * hander,u8 serial,u8 slave_id,u8 port,u32 speed){
    int ret = 0; 
    if(serial !=1 && serial !=2){
        LOG_SYNRON("synron_init error serial should be 1 or 2 1 as A motor 2 as B motor");
        return SYNRON_NOR_ERR;
    }
    if(slave_id>127 || slave_id <2){
        LOG_SYNRON("synron_init error slave_id should be from 1 to 127");   
        return SYNRON_NOR_ERR;
    }
    hander->seria = serial;
    hander->en = 0;
    hander->init = 0;
    hander->slave_id = slave_id;
    hander->port = port;
    hander->speed = speed;
    LOG_SYNRON("seria %d en %d init %d slave_id %d port %d speed %d",hander->seria,hander->en,hander->init,hander->slave_id,hander->port,hander->speed);
    ret = xp_modbus_init(hander->port,(hander->speed),MB_PAR_NONE,1000);
    if(ret != 0){
        LOG_SYNRON("[%s_%d]  xp_modbus_init fail!!!",__FILENAME__,__LINE__);
        return SYNRON_COMMUN_ERR;
    }
    hander->init = 1;
    return SYNRON_SUCCESS;
}

/*
only test 
test_synron()
param:void
return:void
*/
void test_synron(){
    int ret =0;
    //xp_modbus_init(2,115200,MB_PAR_NONE,1000);
    ret = synron_hander_init(&synro_handler_A,1,SYNRON_A_MOTOR_SLAVEID,2,115200);
    if(ret !=SYNRON_SUCCESS ){
        LOG_SYNRON("[%s %d] synron_init fail!!!",__FILE__,__LINE__);
    }   
    ret = synron_hander_init(&synro_handler_B,2,SYNRON_B_MOTOR_SLAVEID,2,115200);
    if(ret != SYNRON_SUCCESS ){
        LOG_SYNRON("[%s %d] synron_init fail!!!",__FILE__,__LINE__);
    }
}
/*

interface of synron modbus driver

*/




/*        
synron_cmd debug 
alios cli modue 
egg:
synron A s 500 : set A motor speed 500 rpm

synron help:

synron+(A or B)+X+Y
A:  A motor
B:  B motor
X:  command code
Y:  value which need to be set 

command

*/

static void synron_cmd(char *buf, int len, int argc, char **argv){

        int arg1 = strtoul(argv[2], NULL, 10);
    	int arg2 = strtoul(argv[3], NULL, 10);
    	int arg3 = strtoul(argv[4], NULL, 10);
        int arg4 = strtoul(argv[5], NULL, 10);
        int temf = 0;
        s16 mode = 0;
        switch(*(argv[1]))
        {
          case 'A':         //AGV小车电机左轮电机
            switch(*(argv[2]))
            {
            case 'I':
                if(arg2 == 2){
                    temf = xp_synron_speed_init(&synro_handler_A);
                    if(temf != SYNRON_SUCCESS){
                        LOG_SYNRON("error synro_handler_A init fail ");
                    }
                    else{
                        LOG_SYNRON("synro_handler_A init success ");
                    }
                     
                }
                if(arg2 == 3){
                    temf = xp_synron_position_init(&synro_handler_A,3);
                    if(temf != SYNRON_SUCCESS){
                        LOG_SYNRON("3 incremental position operation;  synro_handler_A init fail ");
                    }
                    else{
                        LOG_SYNRON("3 incremental position xp_synron_position_init  success ");
                    }
                     
                }
                if(arg2 == 4){
                    temf = xp_synron_position_init(&synro_handler_A,4);
                    if(temf != SYNRON_SUCCESS){
                        LOG_SYNRON("4: absolute position operationerror xp_synron_position_init  fail ");
                    }
                    else{
                        LOG_SYNRON(" 4: absolute position xp_synron_position_init init success ");
                    }
                     
                }
                break;
            case 's':   //set A motor speed value but requir its mode is speedmode;  
                synron_speedmode_setvalue(&synro_handler_A,arg2);
                aos_msleep(2000);
                temf =synron_read_hold(&synro_handler_A,COMMAND_NUM_SET_SPEED);
                LOG_SYNRON("=====>=read set COMMAND_NUM_SET_SPEED %d\n\n",temf);
                if(temf != arg2){
                    LOG_SYNRON("=====>set speed fail!!!");
                }
                else{
                    LOG_SYNRON("=====>set speed success its value is %d!!!",temf);
                }
                
                break;
            case 'e':   //set A moter enbale or unenble 
                if(arg2>0){
                  synron_en(&synro_handler_A,syn_on);
                }
                else{
                  synron_en(&synro_handler_A,syn_off);
                }
                break;
            case 'p':   //set A motor pulse value but requir its mode is incre or abs position mode; 
                temf = 3;//synron_read_hold(&synro_handler_A,PARAMETER_MODE_M_C_CONTROL);
                //aos_msleep(2000);
                if(temf != 3 && temf != 4 ){                   
                   LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
                   break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_handler_A,arg2,arg3);
                }
                break;
            case 'r':   //output motor speed (units rpm)  pulse and so on in their mode.
                mode = synron_read_hold(&synro_handler_A,PARAMETER_MODE_M_C_CONTROL);
                temf = synron_get_speedvalue(&synro_handler_A);
                LOG_SYNRON("=====>the mode is speed mode and its value(rpm) %d",temf);
                temf = synron_get_abs_pulsevalue(&synro_handler_A);
                LOG_SYNRON("======>the mode is %d and absolute position accumulation %d",mode,temf);
                break;
            case 'c':    //0: standby mode; 1: torque operation; 2: speed operation; 3: incremental position operation; 4: absolute position operation
                LOG_SYNRON("=====>choose drive mode and 2 :speed 3: incremental position; 4: absolute position  and it is effective after restarting ");
                temf =synron_mode_driveset(&synro_handler_A,arg2);
                if(temf !=0){
                    LOG_SYNRON("=====>set mode drive faile !");
                }else{
                    LOG_SYNRON("=====>set mode is arg2 %d successful",arg2);
                }
                break;
            case '+':
                temf = synron_speedmode_motor_acc(&synro_handler_A,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_acce faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_acce success !");    
                }
                break;
            case '-':
                 temf = synron_speedmode_motor_dec(&synro_handler_A,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_dec faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_dec success !");    
                }
                break;
            case 'l':
				temf = synron_read_hold(&synro_handler_A,PARAMETER_MODE_M_C_CONTROL);
				LOG_SYNRON("=====>=read set PARAMETER_MODE_M_C_CONTROL %d\n\n",temf);
				 if(temf != 3 && temf != 4 ){                   
               LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
               break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_handler_A,arg2,arg3);
                }
                break;
		    case 'd':
                temf = synron_set_default_rotation(&synro_handler_A,arg2);
                if(temf !=SYNRON_SUCCESS)
                {
                    LOG_SYNRON("error synron_set_default_rotation init fail ");
                }
                LOG_SYNRON("synron_set_default_rotation %d ",arg2);
			case '0':
            temf = synron_speedmode_motor_slow_stop(&synro_handler_A,arg2);  //让左电机慢慢停下
            if(temf !=0)
            {
                LOG_SYNRON("synron_speedmode_motor_slow_stop  A fail ");
            }
            case'W':    //写寄存器特殊值
                LOG_SYNRON("synron A W synron_write_register addr 0x%x, vaule %d",arg2,arg3);
                synron_write_register(&synro_handler_A,arg2,arg3);
                break;
            case'R':    //读寄存器特殊值
                temf = synron_read_hold(&synro_handler_A,arg2);
                LOG_SYNRON("synron A R synron_write_register addr 0x%x  and value is %d \n",arg2,temf);
                break;
            case '!':    //打印当前的故障代码
                temf = synron_get_curfault_code(&synro_handler_A);
                LOG_SYNRON("=====>synron_get_curfault_code %d",temf);
                temf = synron_get_curserious_waring_code(&synro_handler_A);
                LOG_SYNRON("=====>synron_get_curserious_waring_code %d",temf);
                temf = synron_get_curserious_error_code(&synro_handler_A);
                LOG_SYNRON("=====>synron_get_curserious_error_code %d",temf);
                break;
            default:
                break;
            }
            break;
          case 'B':        //AGV小车电机右轮电机
             switch(*(argv[2]))
            {
            case 'I':
                if(arg2 == 2){
                    temf = xp_synron_speed_init(&synro_handler_B);
                    if(temf != 0){
                        LOG_SYNRON("error synro_handler_B init fail ");
                    }
                    else{
                        LOG_SYNRON("synro_handler_B init success ");
                    }
                     
                }
                if(arg2 == 3){
                    temf = xp_synron_position_init(&synro_handler_B,3);
                    if(temf != 0){
                        LOG_SYNRON("3 incremental position operation;  synro_handler_B init fail ");
                    }
                    else{
                        LOG_SYNRON("3 incremental position xp_synron_position_init  success ");
                    }
                     
                }
                if(arg2 == 4){
                    temf = xp_synron_position_init(&synro_handler_B,4);
                    if(temf != SYNRON_SUCCESS){
                        LOG_SYNRON("4: absolute position operationerror xp_synron_position_init  fail ");
                    }
                    else{
                        LOG_SYNRON(" 4: absolute position xp_synron_position_init init success ");
                    }
                     
                }
                break;
            case 's':   //set A motor speed value but requir its mode is speedmode;  
                synron_speedmode_setvalue(&synro_handler_B,arg2);
                aos_msleep(2000);
                temf =synron_read_hold(&synro_handler_B,COMMAND_NUM_SET_SPEED);
                LOG_SYNRON("=====>=read set COMMAND_NUM_SET_SPEED %d\n\n",temf);
                if(temf != arg2){
                    LOG_SYNRON("=====>set speed fail!!!");
                }
                else{
                    LOG_SYNRON("=====>set speed success its value is %d!!!",temf);
                }
                
                break;
            case 'e':   //set A moter enbale or unenble 
                if(arg2>0){
                  synron_en(&synro_handler_B,syn_on);
                }
                else{
                  synron_en(&synro_handler_B,syn_off);
                }
                break;
            case 'p':   //set A motor pulse value but requir its mode is incre or abs position mode; 
                temf = 3;//synron_read_hold(&synro_handler_B,PARAMETER_MODE_M_C_CONTROL);
                //aos_msleep(2000);
                if(temf != 3 && temf != 4 ){                   
                   LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
                   break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_handler_B,arg2,arg3);
                }
                break;
            case 'r':   //output motor speed (units rpm)  pulse and so on in their mode.
                mode = synron_read_hold(&synro_handler_B,PARAMETER_MODE_M_C_CONTROL);
                temf = synron_get_speedvalue(&synro_handler_B);
                LOG_SYNRON("=====>the mode is speed mode and its value(rpm) %d",temf);
                temf = synron_get_abs_pulsevalue(&synro_handler_B);
                LOG_SYNRON("======>the mode is %d and absolute position accumulation %d",mode,temf);
                break;
            case 'c':    //0: standby mode; 1: torque operation; 2: speed operation; 3: incremental position operation; 4: absolute position operation
                LOG_SYNRON("=====>choose drive mode and 2 :speed 3: incremental position; 4: absolute position  and it is effective after restarting ");
                temf =synron_mode_driveset(&synro_handler_B,arg2);
                if(temf !=SYNRON_SUCCESS){
                    LOG_SYNRON("=====>set mode drive faile !");
                }else{
                    LOG_SYNRON("=====>set mode is arg2 %d successful",arg2);
                }
                break;
            case '+':
                temf = synron_speedmode_motor_acc(&synro_handler_B,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_acce faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_acce success !");    
                }
                break;
            case '-':
                 temf = synron_speedmode_motor_dec(&synro_handler_B,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_dec faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_dec success !");    
                }
                break;
            case 'l':
				temf = synron_read_hold(&synro_handler_B,PARAMETER_MODE_M_C_CONTROL);
				LOG_SYNRON("=====>=read set PARAMETER_MODE_M_C_CONTROL %d\n\n",temf);
				 if(temf != 3 && temf != 4 ){                   
               LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
               break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_handler_B,arg2,arg3);
                }
                break;
			case 'd':
                temf = synron_set_default_rotation(&synro_handler_B,arg2);
                if(temf !=SYNRON_SUCCESS)
                {
                    LOG_SYNRON("error synron_set_default_rotation init fail ");
                }
                LOG_SYNRON("synron_set_default_rotation %d ",arg2);	
			case '0':
                temf = synron_speedmode_motor_slow_stop(&synro_handler_B,arg2);  //让左电机慢慢停下
                if(temf !=SYNRON_SUCCESS)
                {
                    LOG_SYNRON("synron_speedmode_motor_slow_stop  B fail ");
                }
            case'W':    //写寄存器特殊值
                LOG_SYNRON("synron B W synron_write_register addr 0x%x, vaule %d",arg2,arg3);
                synron_write_register(&synro_handler_B,arg2,arg3);
                break;
            case'R':    //读寄存器特殊值
                temf = synron_read_hold(&synro_handler_B,arg2);
                LOG_SYNRON("synron B R synron_write_register addr 0x%x  and value is %d \n",arg2,temf);
                break;
            case '!':    //打印当前的故障代码
                temf = synron_get_curfault_code(&synro_handler_B);
                LOG_SYNRON("=====>synron_get_curfault_code %d",temf);
                temf = synron_get_curserious_waring_code(&synro_handler_B);
                LOG_SYNRON("=====>synron_get_curserious_waring_code %d",temf);
                temf = synron_get_curserious_error_code(&synro_handler_B);
                LOG_SYNRON("=====>synron_get_curserious_error_code %d",temf);
                break;
            default:
                break;
            }
            break;
          case 'S':
            switch(*(argv[2]))
            {   
                case 'i':
                    temf =xp_synron_init(arg2, arg3, arg4);
                    if(temf !=SYNRON_SUCCESS){
                        LOG_SYNRON("xp_synron_init faile mode %d port %d baudrate %d",arg2,arg3,arg4);
                    }
                    else{                    
                        LOG_SYNRON("xp_synron_init success mode %d port %d baudrate %d",arg2,arg3,arg4);
                    }
                    break;
                case 's':
                     synron_speedmode_setvalue(&synro_handler_A,arg2);
                     synron_speedmode_setvalue(&synro_handler_B,arg2);
                break;
                case 'p':
                    synron_position_setvalue(&synro_handler_A,arg2,arg3);
                    synron_position_setvalue(&synro_handler_B,arg2,arg3);
                break;
                case '+':
                temf = synron_speedmode_motor_acc(&synro_handler_A,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_acce faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_acce success !");    
                }   
                temf = synron_speedmode_motor_acc(&synro_handler_B,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_acce faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_acce success !");    
                }
                break;
                case '-':
                temf = synron_speedmode_motor_dec(&synro_handler_A,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_dec faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_dec success !");    
                }
                  temf = synron_speedmode_motor_dec(&synro_handler_B,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_dec faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_dec success !");    
                }                
                break;
                case 'r':
                mode = synron_read_hold(&synro_handler_A,PARAMETER_MODE_M_C_CONTROL);
                temf = synron_get_speedvalue(&synro_handler_A);
                LOG_SYNRON("A=====>the mode is speed mode and its value(rpm) %d",temf);
                temf = synron_get_abs_pulsevalue(&synro_handler_A);
                LOG_SYNRON("A======>the mode is %d and absolute position accumulation %d",mode,temf);               
                mode = synron_read_hold(&synro_handler_B,PARAMETER_MODE_M_C_CONTROL);
                temf = synron_get_speedvalue(&synro_handler_B);
                LOG_SYNRON("B=====>the mode is speed mode and its value(rpm) %d",temf);
                temf = synron_get_abs_pulsevalue(&synro_handler_B);
                LOG_SYNRON("B======>the mode is %d and absolute position accumulation %d",mode,temf);
                break;
                case '0':
                temf = synron_speedmode_motor_slow_stop(&synro_handler_A,arg2);  //让左电机慢慢停下
                if(temf !=0)
                {
                    LOG_SYNRON("synron_speedmode_motor_slow_stop A fail ");
                }
                 temf = synron_speedmode_motor_slow_stop(&synro_handler_B,arg2);  //让左电机慢慢停下
                if(temf !=0)
                {
                    LOG_SYNRON("synron_speedmode_motor_slow_stop  B fail ");
                }
                default:
                    break;
            }
            break;
          case 'C':        //电缸电机左电机
               switch(*(argv[2]))
            {
            case 'I':
                if(arg2 == 2){
                    temf = xp_synron_speed_init(&synro_lifter_handler_A);
                    if(temf != SYNRON_SUCCESS){
                        LOG_SYNRON("error synro_lifter_handler_A init fail ");
                    }
                    else{
                        LOG_SYNRON("synro_lifter_handler_A init success ");
                    }
                     
                }
                if(arg2 == 3){
                    temf = xp_synron_position_init(&synro_lifter_handler_A,3);
                    if(temf != SYNRON_SUCCESS){
                        LOG_SYNRON("3 incremental position operation;  synro_lifter_handler_A init fail ");
                    }
                    else{
                        LOG_SYNRON("3 incremental position xp_synron_position_init  success ");
                    }
                     
                }
                if(arg2 == 4){
                    temf = xp_synron_position_init(&synro_lifter_handler_A,4);
                    if(temf != SYNRON_SUCCESS){
                        LOG_SYNRON("4: absolute position operationerror xp_synron_position_init  fail ");
                    }
                    else{
                        LOG_SYNRON(" 4: absolute position xp_synron_position_init init success ");
                    }
                     
                }
                break;
            case 's':   //set A motor speed value but requir its mode is speedmode;  
                synron_speedmode_setvalue(&synro_lifter_handler_A,arg2);
                aos_msleep(2000);
                temf =synron_read_hold(&synro_lifter_handler_A,COMMAND_NUM_SET_SPEED);
                LOG_SYNRON("=====>=read set COMMAND_NUM_SET_SPEED %d\n\n",temf);
                if(temf != arg2){
                    LOG_SYNRON("=====>set speed fail!!!");
                }
                else{
                    LOG_SYNRON("=====>set speed success its value is %d!!!",temf);
                }
                
                break;
            case 'e':   //set A moter enbale or unenble 
                if(arg2>0){
                  synron_en(&synro_lifter_handler_A,syn_on);
                }
                else{
                  synron_en(&synro_lifter_handler_A,syn_off);
                }
                break;
            case 'p':   //set A motor pulse value but requir its mode is incre or abs position mode; 
                temf = 3;//synron_read_hold(&synro_lifter_handler_A,PARAMETER_MODE_M_C_CONTROL);
                //aos_msleep(2000);
                if(temf != 3 && temf != 4 ){                   
                   LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
                   break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_lifter_handler_A,arg2,arg3);
                }
                break;
            case 'r':   //output motor speed (units rpm)  pulse and so on in their mode.
                mode = synron_read_hold(&synro_lifter_handler_A,PARAMETER_MODE_M_C_CONTROL);
                temf = synron_get_speedvalue(&synro_lifter_handler_A);
                LOG_SYNRON("=====>the mode is speed mode and its value(rpm) %d",temf);
                temf = synron_get_abs_pulsevalue(&synro_lifter_handler_A);
                LOG_SYNRON("======>the mode is %d and absolute position accumulation %d",mode,temf);
                break;
            case 'c':    //0: standby mode; 1: torque operation; 2: speed operation; 3: incremental position operation; 4: absolute position operation
                LOG_SYNRON("=====>choose drive mode and 2 :speed 3: incremental position; 4: absolute position  and it is effective after restarting ");
                temf =synron_mode_driveset(&synro_lifter_handler_A,arg2);
                if(temf !=SYNRON_SUCCESS){
                    LOG_SYNRON("=====>set mode drive faile !");
                }else{
                    LOG_SYNRON("=====>set mode is arg2 %d successful",arg2);
                }
                break;
            case '+':
                temf = synron_speedmode_motor_acc(&synro_lifter_handler_A,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_acce faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_acce success !");    
                }
                break;
            case '-':
                 temf = synron_speedmode_motor_dec(&synro_lifter_handler_A,arg2,arg3 );
                if(temf != SYNRON_SUCCESS){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_dec faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_dec success !");    
                }
                break;
            case 'l':
				temf = synron_read_hold(&synro_lifter_handler_A,PARAMETER_MODE_M_C_CONTROL);
				LOG_SYNRON("=====>=read set PARAMETER_MODE_M_C_CONTROL %d\n\n",temf);
				 if(temf != 3 && temf != 4 ){                   
               LOG_SYNRON("pulse value but requir its mode is incre or abs position mode!!!"); 
               break;
                }
                else {                    
                    LOG_SYNRON("pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_lifter_handler_A,arg2,arg3);
                }
                break;
			case 'd':
                temf = synron_set_default_rotation(&synro_lifter_handler_A,arg2);
                if(temf !=SYNRON_SUCCESS)
                {
                    LOG_SYNRON("error synron_set_default_rotation init fail ");
                }
                LOG_SYNRON("synron_set_default_rotation %d ",arg2);	
			case '0':
                temf = synron_speedmode_motor_slow_stop(&synro_lifter_handler_A,arg2);  //让左电机慢慢停下
                if(temf !=SYNRON_SUCCESS)
                {
                    LOG_SYNRON("synron_speedmode_motor_slow_stop  B fail ");
                }
            case'W':    //写寄存器特殊值
                LOG_SYNRON("synro_lifter_handler_A W synron_write_register addr 0x%x, vaule %d",arg2,arg3);
                synron_write_register(&synro_lifter_handler_A,arg2,arg3);
                break;
            case'R':    //读寄存器特殊值
                temf = synron_read_hold(&synro_lifter_handler_A,arg2);
                LOG_SYNRON("synro_lifter_handler_A  R synron_write_register addr 0x%x  and value is %d \n",arg2,temf);
                break;
            case '!':    //打印当前的故障代码
                temf = synron_get_curfault_code(&synro_lifter_handler_A);
                LOG_SYNRON("synron_get_curfault_code %d",temf);
                temf = synron_get_curserious_waring_code(&synro_lifter_handler_A);
                LOG_SYNRON("synron_get_curserious_waring_code %d",temf);
                temf = synron_get_curserious_error_code(&synro_lifter_handler_A);
                LOG_SYNRON("synron_get_curserious_error_code %d",temf);
                break;
            default:
                break;
            }
            break;
          case 'D':       //电缸电机右电机
               switch(*(argv[2]))
            {
            case 'I':
                if(arg2 == 2){
                    temf = xp_synron_speed_init(&synro_lifter_handler_B);
                    if(temf != 0){
                        LOG_SYNRON("error synro_lifter_handler_B init fail ");
                    }
                    else{
                        LOG_SYNRON("synro_lifter_handler_B init success ");
                    }
                     
                }
                if(arg2 == 3){
                    temf = xp_synron_position_init(&synro_lifter_handler_B,3);
                    if(temf != 0){
                        LOG_SYNRON("3 incremental position operation;  synro_lifter_handler_B init fail ");
                    }
                    else{
                        LOG_SYNRON("3 incremental position xp_synron_position_init  success ");
                    }
                     
                }
                if(arg2 == 4){
                    temf = xp_synron_position_init(&synro_lifter_handler_B,4);
                    if(temf != 0){
                        LOG_SYNRON("4: absolute position operationerror xp_synron_position_init  fail ");
                    }
                    else{
                        LOG_SYNRON(" 4: absolute position xp_synron_position_init init success ");
                    }
                     
                }
                break;
            case 's':   //set A motor speed value but requir its mode is speedmode;  
                synron_speedmode_setvalue(&synro_lifter_handler_B,arg2);
                aos_msleep(2000);
                temf =synron_read_hold(&synro_lifter_handler_B,COMMAND_NUM_SET_SPEED);
                LOG_SYNRON("=====>=read set COMMAND_NUM_SET_SPEED %d\n\n",temf);
                if(temf != arg2){
                    LOG_SYNRON("=====>set speed fail!!!");
                }
                else{
                    LOG_SYNRON("=====>set speed success its value is %d!!!",temf);
                }
                
                break;
            case 'e':   //set A moter enbale or unenble 
                if(arg2>0){
                  synron_en(&synro_lifter_handler_B,syn_on);
                }
                else{
                  synron_en(&synro_lifter_handler_B,syn_off);
                }
                break;
            case 'p':   //set A motor pulse value but requir its mode is incre or abs position mode; 
                temf = 3;//synron_read_hold(&synro_lifter_handler_B,PARAMETER_MODE_M_C_CONTROL);
                //aos_msleep(2000);
                if(temf != 3 && temf != 4 ){                   
                   LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
                   break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_lifter_handler_B,arg2,arg3);
                }
                break;
            case 'r':   //output motor speed (units rpm)  pulse and so on in their mode.
                mode = synron_read_hold(&synro_lifter_handler_B,PARAMETER_MODE_M_C_CONTROL);
                temf = synron_get_speedvalue(&synro_lifter_handler_B);
                LOG_SYNRON("=====>the mode is speed mode and its value(rpm) %d",temf);
                temf = synron_get_abs_pulsevalue(&synro_lifter_handler_B);
                LOG_SYNRON("======>the mode is %d and absolute position accumulation %d",mode,temf);
                break;
            case 'c':    //0: standby mode; 1: torque operation; 2: speed operation; 3: incremental position operation; 4: absolute position operation
                LOG_SYNRON("=====>choose drive mode and 2 :speed 3: incremental position; 4: absolute position  and it is effective after restarting ");
                temf =synron_mode_driveset(&synro_lifter_handler_B,arg2);
                if(temf !=0){
                    LOG_SYNRON("=====>set mode drive faile !");
                }else{
                    LOG_SYNRON("=====>set mode is arg2 %d successful",arg2);
                }
                break;
            case '+':
                temf = synron_speedmode_motor_acc(&synro_lifter_handler_B,arg2,arg3 );
                if(temf != 0){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_acce faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_acce success !");    
                }
                break;
            case '-':
                 temf = synron_speedmode_motor_dec(&synro_lifter_handler_B,arg2,arg3 );
                if(temf != 0){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_dec faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_dec success !");    
                }
                break;
            case 'l':
				temf = synron_read_hold(&synro_lifter_handler_B,PARAMETER_MODE_M_C_CONTROL);
				LOG_SYNRON("=====>=read set PARAMETER_MODE_M_C_CONTROL %d\n\n",temf);
				 if(temf != 3 && temf != 4 ){                   
               LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
               break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_lifter_handler_B,arg2,arg3);
                }
                break;
			case 'd':
                temf = synron_set_default_rotation(&synro_lifter_handler_B,arg2);
                if(temf !=0)
                {
                    LOG_SYNRON("error synron_set_default_rotation init fail ");
                }
                LOG_SYNRON("synron_set_default_rotation %d ",arg2);	
			case '0':
                temf = synron_speedmode_motor_slow_stop(&synro_lifter_handler_B,arg2);  //让左电机慢慢停下
                if(temf !=0)
                {
                    LOG_SYNRON("synron_speedmode_motor_slow_stop  B fail ");
                }
            case'W':    //写寄存器特殊值
                LOG_SYNRON("synron B W synron_write_register addr 0x%x, vaule %d",arg2,arg3);
                synron_write_register(&synro_lifter_handler_B,arg2,arg3);
                break;
            case'R':    //读寄存器特殊值
                temf = synron_read_hold(&synro_lifter_handler_B,arg2);
                LOG_SYNRON("synro_lifter_handler_B R synron_write_register addr 0x%x  and value is %d \n",arg2,temf);
                break;
             case '!':    //打印当前的故障代码
                temf = synron_get_curfault_code(&synro_lifter_handler_B);
                LOG_SYNRON("=====>synron_get_curfault_code %d",temf);
                temf = synron_get_curserious_waring_code(&synro_lifter_handler_B);
                LOG_SYNRON("=====>synron_get_curserious_waring_code %d",temf);
                temf = synron_get_curserious_error_code(&synro_lifter_handler_B);
                LOG_SYNRON("=====>synron_get_curserious_error_code %d",temf);
                break;
            default:
                break;
            }
            break;
          case 'E':        //铰接电机
               switch(*(argv[2]))
            {
            case 'I':
                if(arg2 == 2){
                    temf = xp_synron_speed_init(&synro_brush_hinge_hander);
                    if(temf != 0){
                        LOG_SYNRON("error synro_brush_hinge_hander init fail ");
                    }
                    else{
                        LOG_SYNRON("synro_brush_hinge_hander init success ");
                    }
                     
                }
                if(arg2 == 3){
                    temf = xp_synron_position_init(&synro_brush_hinge_hander,3);
                    if(temf != 0){
                        LOG_SYNRON("3 incremental position operation;  synro_brush_hinge_hander init fail ");
                    }
                    else{
                        LOG_SYNRON("3 incremental position xp_synron_position_init  success ");
                    }
                     
                }
                if(arg2 == 4){
                    temf = xp_synron_position_init(&synro_brush_hinge_hander,4);
                    if(temf != 0){
                        LOG_SYNRON("4: absolute position operationerror xp_synron_position_init  fail ");
                    }
                    else{
                        LOG_SYNRON(" 4: absolute position xp_synron_position_init init success ");
                    }
                     
                }
                break;
            case 's':   //set A motor speed value but requir its mode is speedmode;  
                synron_speedmode_setvalue(&synro_brush_hinge_hander,arg2);
                aos_msleep(2000);
                temf =synron_read_hold(&synro_brush_hinge_hander,COMMAND_NUM_SET_SPEED);
                LOG_SYNRON("=====>=read set COMMAND_NUM_SET_SPEED %d\n\n",temf);
                if(temf != arg2){
                    LOG_SYNRON("=====>set speed fail!!!");
                }
                else{
                    LOG_SYNRON("=====>set speed success its value is %d!!!",temf);
                }
                
                break;
            case 'e':   //set A moter enbale or unenble 
                if(arg2>0){
                  synron_en(&synro_brush_hinge_hander,syn_on);
                }
                else{
                  synron_en(&synro_brush_hinge_hander,syn_off);
                }
                break;
            case 'p':   //set A motor pulse value but requir its mode is incre or abs position mode; 
                temf = 3;//synron_read_hold(&synro_brush_hinge_hander,PARAMETER_MODE_M_C_CONTROL);
                //aos_msleep(2000);
                if(temf != 3 && temf != 4 ){                   
                   LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
                   break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_brush_hinge_hander,arg2,arg3);
                }
                break;
            case 'r':   //output motor speed (units rpm)  pulse and so on in their mode.
                mode = synron_read_hold(&synro_brush_hinge_hander,PARAMETER_MODE_M_C_CONTROL);
                temf = synron_get_speedvalue(&synro_brush_hinge_hander);
                LOG_SYNRON("=====>the mode is speed mode and its value(rpm) %d",temf);
                temf = synron_get_abs_pulsevalue(&synro_brush_hinge_hander);
                LOG_SYNRON("======>the mode is %d and absolute position accumulation %d",mode,temf);
                break;
            case 'c':    //0: standby mode; 1: torque operation; 2: speed operation; 3: incremental position operation; 4: absolute position operation
                LOG_SYNRON("=====>choose drive mode and 2 :speed 3: incremental position; 4: absolute position  and it is effective after restarting ");
                temf =synron_mode_driveset(&synro_brush_hinge_hander,arg2);
                if(temf !=0){
                    LOG_SYNRON("=====>set mode drive faile !");
                }else{
                    LOG_SYNRON("=====>set mode is arg2 %d successful",arg2);
                }
                break;
            case '+':
                temf = synron_speedmode_motor_acc(&synro_brush_hinge_hander,arg2,arg3 );
                if(temf != 0){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_acce faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_acce success !");    
                }
                break;
            case '-':
                 temf = synron_speedmode_motor_dec(&synro_brush_hinge_hander,arg2,arg3 );
                if(temf != 0){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_dec faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_dec success !");    
                }
                break;
            case 'l':
				temf = synron_read_hold(&synro_brush_hinge_hander,PARAMETER_MODE_M_C_CONTROL);
				LOG_SYNRON("=====>=read set PARAMETER_MODE_M_C_CONTROL %d\n\n",temf);
				 if(temf != 3 && temf != 4 ){                   
               LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
               break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_brush_hinge_hander,arg2,arg3);
                }
                break;
			case 'd':
                temf = synron_set_default_rotation(&synro_brush_hinge_hander,arg2);
                if(temf !=0)
                {
                    LOG_SYNRON("error synron_set_default_rotation init fail ");
                }
                LOG_SYNRON("synron_set_default_rotation %d ",arg2);	
			case '0':
                temf = synron_speedmode_motor_slow_stop(&synro_brush_hinge_hander,arg2);  //让左电机慢慢停下
                if(temf !=0)
                {
                    LOG_SYNRON("synron_speedmode_motor_slow_stop  B fail ");
                }
            case'W':    //写寄存器特殊值
                LOG_SYNRON("synron B W synron_write_register addr 0x%x, vaule %d",arg2,arg3);
                synron_write_register(&synro_brush_hinge_hander,arg2,arg3);
                break;
            case'R':    //读寄存器特殊值
                temf = synron_read_hold(&synro_brush_hinge_hander,arg2);
                LOG_SYNRON("synron B R synron_write_register addr 0x%x  and value is %d \n",arg2,temf);
                break;
           case '!':    //打印当前的故障代码
                temf = synron_get_curfault_code(&synro_brush_hinge_hander);
                LOG_SYNRON("synron_get_curfault_code %d",temf);
                temf = synron_get_curserious_waring_code(&synro_brush_hinge_hander);
                LOG_SYNRON("synron_get_curserious_waring_code %d",temf);
                temf = synron_get_curserious_error_code(&synro_brush_hinge_hander);
                LOG_SYNRON("synron_get_curserious_error_code %d",temf);
                break;
            default:
                break;
            }
            break;
          case 'F':        //刷子电机
                switch(*(argv[2]))
            {
            case 'I':
                if(arg2 == 2){
                    temf = xp_synron_speed_init(&synro_brush_handler);
                    if(temf != 0){
                        LOG_SYNRON("error synro_brush_handler init fail ");
                    }
                    else{
                        LOG_SYNRON("synro_brush_handler init success ");
                    }
                     
                }
                if(arg2 == 3){
                    temf = xp_synron_position_init(&synro_brush_handler,3);
                    if(temf != 0){
                        LOG_SYNRON("3 incremental position operation;  synro_brush_handler init fail ");
                    }
                    else{
                        LOG_SYNRON("3 incremental position xp_synron_position_init  success ");
                    }
                     
                }
                if(arg2 == 4){
                    temf = xp_synron_position_init(&synro_brush_handler,4);
                    if(temf != 0){
                        LOG_SYNRON("4: absolute position operationerror xp_synron_position_init  fail ");
                    }
                    else{
                        LOG_SYNRON(" 4: absolute position xp_synron_position_init init success ");
                    }
                     
                }
                break;
            case 's':   //set A motor speed value but requir its mode is speedmode;  
                synron_speedmode_setvalue(&synro_brush_handler,arg2);
                aos_msleep(2000);
                temf =synron_read_hold(&synro_brush_handler,COMMAND_NUM_SET_SPEED);
                LOG_SYNRON("=====>=read set COMMAND_NUM_SET_SPEED %d\n\n",temf);
                if(temf != arg2){
                    LOG_SYNRON("=====>set speed fail!!!");
                }
                else{
                    LOG_SYNRON("=====>set speed success its value is %d!!!",temf);
                }
                
                break;
            case 'e':   //set A moter enbale or unenble 
                if(arg2>0){
                  synron_en(&synro_brush_handler,syn_on);
                }
                else{
                  synron_en(&synro_brush_handler,syn_off);
                }
                break;
            case 'p':   //set A motor pulse value but requir its mode is incre or abs position mode; 
                temf = 3;//synron_read_hold(&synro_brush_handler,PARAMETER_MODE_M_C_CONTROL);
                //aos_msleep(2000);
                if(temf != 3 && temf != 4 ){                   
                   LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
                   break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_brush_handler,arg2,arg3);
                }
                break;
            case 'r':   //output motor speed (units rpm)  pulse and so on in their mode.
                mode = synron_read_hold(&synro_brush_handler,PARAMETER_MODE_M_C_CONTROL);
                temf = synron_get_speedvalue(&synro_brush_handler);
                LOG_SYNRON("=====>the mode is speed mode and its value(rpm) %d",temf);
                temf = synron_get_abs_pulsevalue(&synro_brush_handler);
                LOG_SYNRON("======>the mode is %d and absolute position accumulation %d",mode,temf);
                break;
            case 'c':    //0: standby mode; 1: torque operation; 2: speed operation; 3: incremental position operation; 4: absolute position operation
                LOG_SYNRON("=====>choose drive mode and 2 :speed 3: incremental position; 4: absolute position  and it is effective after restarting ");
                temf =synron_mode_driveset(&synro_brush_handler,arg2);
                if(temf !=0){
                    LOG_SYNRON("=====>set mode drive faile !");
                }else{
                    LOG_SYNRON("=====>set mode is arg2 %d successful",arg2);
                }
                break;
            case '+':
                temf = synron_speedmode_motor_acc(&synro_brush_handler,arg2,arg3 );
                if(temf != 0){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_acce faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_acce success !");    
                }
                break;
            case '-':
                 temf = synron_speedmode_motor_dec(&synro_brush_handler,arg2,arg3 );
                if(temf != 0){                  
                    LOG_SYNRON("=====>synron_speedmode_motor_dec faile !");
                }
                else{
                    LOG_SYNRON("=====>synron_speedmode_motor_dec success !");    
                }
                break;
            case 'l':
				temf = synron_read_hold(&synro_brush_handler,PARAMETER_MODE_M_C_CONTROL);
				LOG_SYNRON("=====>=read set PARAMETER_MODE_M_C_CONTROL %d\n\n",temf);
				 if(temf != 3 && temf != 4 ){                   
               LOG_SYNRON("=====>pulse value but requir its mode is incre or abs position mode!!!"); 
               break;
                }
                else {                    
                    LOG_SYNRON("=====>pulse value is %d need time is %d ms!!!",arg2,arg3); 
                    synron_position_setvalue(&synro_brush_handler,arg2,arg3);
                }
                break;
			case 'd':
                temf = synron_set_default_rotation(&synro_brush_handler,arg2);
                if(temf !=0)
                {
                    LOG_SYNRON("error synron_set_default_rotation init fail ");
                }
                LOG_SYNRON("synron_set_default_rotation %d ",arg2);	
			case '0':
                temf = synron_speedmode_motor_slow_stop(&synro_brush_handler,arg2);  //让左电机慢慢停下
                if(temf !=0)
                {
                    LOG_SYNRON("synron_speedmode_motor_slow_stop  B fail ");
                }
            case'W':    //写寄存器特殊值
                LOG_SYNRON("synron B W synron_write_register addr 0x%x, vaule %d",arg2,arg3);
                synron_write_register(&synro_brush_handler,arg2,arg3);
                break;
            case'R':    //读寄存器特殊值
                temf = synron_read_hold(&synro_brush_handler,arg2);
                LOG_SYNRON("synron B R synron_write_register addr 0x%x  and value is %d \n",arg2,temf);
                break;
            case '!':    //打印当前的故障代码
                temf = synron_get_curfault_code(&synro_brush_handler);
                LOG_SYNRON("=====>synron_get_curfault_code %d",temf);
                temf = synron_get_curserious_waring_code(&synro_brush_handler);
                LOG_SYNRON("=====>synron_get_curserious_waring_code %d",temf);
                temf = synron_get_curserious_error_code(&synro_brush_handler);
                LOG_SYNRON("=====>synron_get_curserious_error_code %d",temf);
                break;
            default:
                break;
            }
            break;
          case 'h':
            printf("\n \
synron cli help:\n  \
\n \
synron+(A or B or S)+X+Y\n \
A:  A motor\n  \
B:  B motor\n  \
X:  command code\n  \
Y:  value which need to be set\n  \
command example:\n  \
\n   \
synron A I 2    将A电机初始化为2 速度模式\n \
synron A I 3    将A电机初始化为3 增量位置模式\n \
synron A I 4    将A电机初始化为4 绝对位置模式\n \
synron A s 500 设置A 电机500rpm  正转 \
synron A s -500 设置A 电机-500rpm 相反方向 \
\n \
synron A e 1 设置A电机使能\n \
synron A e 0 设置A电机不使能\n \
\n  \
synron A c 2 设置A电机为速度模式\n  \
synron B c 4 设置A电机为绝对位置模式\n  \
(0: standby mode; 1: torque operation; 2: speed operation; \n  \
3: incremental position operation; 4: absolute position operation)\n  \
\n \
synron A p 10000 500 设置A电机为10000脉冲 时间为500ms 绝对位置和相对位置会自己判断  数字正为正转\n \
\n  \
synron A r 读取当前的信息转速 绝对位置脉冲\n  \
synron A r 读取当前的信息转速 绝对位置脉冲\n  \
\n  \
synron S i 2  2 115200  速度模式 端口2 进行初始化 115200波特率 同时AB\n   \
synron S s 200   同时让A B两个转 前提是分别设置为速度模式\n   \
synron S p 10000 500  位置模式下 同时让A B两个电机10000脉冲 需要500ms\n   \
synron S r  同时读取 A B的两个电机位置模式和速度模式的信息\n");
            break;            
          default:            
            LOG_SYNRON("synron_cmd  argv[1] is error ,and shuold be is 'A' or 'B'!");
            break;
        }
}


static struct cli_command synron[] = {
     { "synron","interface of synron modbus driver to debug :\r\n",synron_cmd},
 };

/*
alios cli module register synron
*/
void synron_cli_init(void){
    if(aos_cli_register_commands(&synron[0],1)==0){
        LOG_SYNRON("Cli synron_cli_ini debug access success !\r\n");
    }
    else{
        LOG_SYNRON("Cli synron_cli_ini debug access fail !\r\n");
    }
}
//////////////////////////////////////////////////////////////////////////////
//The following functions can be called directly by the user
/*

void xp_synron_speed_init()
param:
     handler:motor object
return 
    0 sucess
    SYNRON_COMMUN_ERR faile


*/

int xp_synron_speed_init(SYNRO_handler * hander){
    int ret =0;
    ret = synron_en(hander,syn_on);
    if(ret != SYNRON_SUCCESS){
        LOG_SYNRON("[%s_%d] synron_en fail!!!");
        return SYNRON_COMMUN_ERR;
    }
    return synron_mode_driveset(hander,2);
}

/*

int xp_synron_position_init()
param:
     handler:motor object
     u8 mode  3
return 
    0  sucecss
    SYNRON_COMMUN_ERR faile
0：待机模式； 1：转矩运行； 2：速度运行； 3：增量位置运行； 4:绝对位置运行  Modbus/CAN 总线控制模式下
0: standby mode;
1: torque operation mode
2: speed operation mode;
3: the incremental position runs according to time (DeltaTime/DeltaPulse);
4: absolute position runs according to time (DeltaTime/AbsPulse);
5: incremental position protocol speed mode (TargetSpeed/DeltaPulse);
6: absolute position protocol speed mode (TargetSpeed /AbsPulse)
总线控制模式(包括 ModBus 和 CAN)：
0：待机方式；
1：转矩运行模式
2：速度运行方式；
3：增量位置按时间运行模式(DeltaTime/DeltaPulse)；
4：绝对位置按时间运行模式(DeltaTime/AbsPulse)；
5：增量位置协议速度模式(TargetSpeed/DeltaPulse)；
6：绝对位置协议速度模式(TargetSpeed /AbsPulse)


*/

int  xp_synron_position_init(SYNRO_handler * hander,u8 mode){
    int ret =0;
    if(mode !=3 && mode!= 4 && mode!=5 &&mode!=6){
        LOG_SYNRON("mode value is error!");
        return SYNRON_NOR_ERR;
    }
    ret = synron_en(hander,syn_on);
    if(ret != SYNRON_SUCCESS){
        LOG_SYNRON("[%s_%d] synron_en fail!!!",__FILENAME__,__LINE__);
         return SYNRON_COMMUN_ERR;
    }
     return synron_mode_driveset(hander,mode);
    
}
/*

int xp_synron_init(u8 mode)
param:
    mode :  1：转矩运行(暂不支持)； 2：速度运行； 3：增量位置运行； 4:绝对位置运行
    port :  串口号
    speed ：波特率
return: 
     0  success
     SYNRON_COMMUN_ERR  fail   
*/
int xp_synron_init(u8 mode,u8 port,u32 speed){
    s16 ret =0;
    if((mode!=2)&&(mode!=3)&&(mode !=4)){
        LOG_SYNRON(" parameter mode is error ,its shuold be 1 ,2 ,3 or 4,but 1 is not supported now ! ");
        return SYNRON_COMMUN_ERR;
    }
    ret = synron_hander_init(&synro_handler_A,1,SYNRON_A_MOTOR_SLAVEID,port,speed);
    if(ret != 0 ){
        LOG_SYNRON("[%s_%d] synro_handler_A synron_hander_init fail!!!",__FILENAME__,__LINE__);
        return SYNRON_COMMUN_ERR;
    } 
   ret = synron_hander_init(&synro_handler_B,2,SYNRON_B_MOTOR_SLAVEID,port,speed);
    if(ret != 0 ){
        LOG_SYNRON("[%s_%d]  synro_handler_Bsynron_hander_init!!!",__FILENAME__,__LINE__);
        return SYNRON_COMMUN_ERR;
    }
    /*
        设置工作模式为modbus不然会有问题
    */
    synron_mode_commset(&synro_handler_A,2);
    synron_mode_commset(&synro_handler_B,2);
    if(mode == 2){
       ret = xp_synron_speed_init(&synro_handler_A);
       if(ret != 0){
           LOG_SYNRON("[%s_%d]  xp_synron_speed_init(&synro_handler_A) fail!!!",__FILENAME__,__LINE__);           
           return SYNRON_COMMUN_ERR;
       }
       ret = xp_synron_speed_init(&synro_handler_B);
       if(ret != 0){
           LOG_SYNRON("[%s_%d]  xp_synron_speed_init(&synro_handler_B) fail!!!",__FILENAME__,__LINE__);           
           return SYNRON_COMMUN_ERR;
       }
       ret = synron_speedmode_acceleratetime(&synro_handler_A,synro_handler_A.config.acc_time);
        if(ret != 0){
           LOG_SYNRON("[%s_%d]  synron_speedmode_acceleratetime(&synro_handler_A) fail!!!",__FILENAME__,__LINE__);           
           return SYNRON_COMMUN_ERR;
       }
        ret = synron_speedmode_deceleratetime(&synro_handler_A,synro_handler_A.config.dcc_time);
        if(ret != 0){
           LOG_SYNRON("[%s_%d]  synron_speedmode_deceleratetime(&synro_handler_A) fail!!!",__FILENAME__,__LINE__);           
           return SYNRON_COMMUN_ERR;
       }
        ret = synron_speedmode_acceleratetime(&synro_handler_B,synro_handler_B.config.acc_time);
        if(ret != 0){
           LOG_SYNRON("[%s_%d]  synron_speedmode_acceleratetime(&synro_handler_B) fail!!!",__FILENAME__,__LINE__);           
           return SYNRON_COMMUN_ERR;
       }
        ret = synron_speedmode_deceleratetime(&synro_handler_B,synro_handler_B.config.dcc_time);
        if(ret != 0){
           LOG_SYNRON("[%s_%d]  synron_speedmode_deceleratetime(&synro_handler_B) fail!!!",__FILENAME__,__LINE__);           
           return SYNRON_COMMUN_ERR;
       }
       
    }
    if(mode == 3 || mode == 4){
       ret = xp_synron_position_init(&synro_handler_A,mode);
       if(ret != 0){
           LOG_SYNRON("[%s_%d]  xp_synron_position_init(&synro_handler_A) fail!!! mode %d",__FILENAME__,__LINE__,mode);           
           return SYNRON_COMMUN_ERR;
       }
       ret = xp_synron_position_init(&synro_handler_B,mode);
       if(ret != 0){
           LOG_SYNRON("[%s_%d]  xp_synron_position_init(&synro_handler_B) fail!!! mode %d",__FILENAME__,__LINE__,mode);           
           return SYNRON_COMMUN_ERR;
       }
    }
    return SYNRON_SUCCESS;
    
    
}




